/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: CoordinateDriveClient.hpp,v 1.24 2009-12-08 10:48:27 morsko Exp $
 *
 * \file
 * \brief MaCI - CoordinateDrive Interface Client header
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 */
#ifndef _MACI_INTERFACE_COORDINATEDRIVECLIENT_HPP_
#define _MACI_INTERFACE_COORDINATEDRIVECLIENT_HPP_

#include "MaCI.hpp"
#include "CoordinateDriveData.hpp"
#include "thread.hpp"
#include "sync.hpp"
#include <map>


namespace MaCI {
  namespace CoordinateDrive {

    typedef std::vector<TCoordinate2D> TCoordinateArray;

    typedef std::vector<TDriveMode> TDriveModeArray;


    /** std::vector wrapper for TCoordinate2D elements.
     *
     * This container hides the template parameters of STL classes,
     * allowing Python wrappers to handle the elements more
     * easily. NOTE: In C++ applications, there is no sense in using
     * this container, as all functionality is already built in the
     * STL container class 'std::vector'.
     *
     * \note This could have been easily implemeted as template class
     * to allow using it in other interfaces too, but as the main goal
     * was to hide this behaviour, this class needs to be explicitly
     * defined on per type basis.
     */
    class TCoordinateContainer
    {
    public:
      /** Default, empty constructor.
       *
       */
      TCoordinateContainer(void) 
        : array() {}


      /** Copy from TCoordinate2D vector.
       */
      TCoordinateContainer(const std::vector< TCoordinate2D > &aArray) 
        : array(aArray) {}
      
      
      /** Array of elements of type 'TCoordinate2D'.
       */
      std::vector< TCoordinate2D > array;
      
      
      /** Get array size in elements.
       *
       * This function returns the number of elements stored in this
       * container.
       *
       * @return                    Number of elements in the container.
       */
      inline const unsigned int GetArraySize(void) const {
        return array.size();
      }
      
      
      /** Return single array entry.
       *
       * This function returns single array entry by index.
       *
       * \note This function returns a constant reference to
       * object. Therefore, this element is only valid if the
       * underlying container (std::vector< TCoordinate2D >) remains
       * unchanged.
       * 
       * @return Constant reference to single array entry.
       *
       */
      inline const TCoordinate2D &GetArrayElement(const unsigned int aIndex) {
        return array[aIndex];
      }


      /** Clear associated array.
       */
      inline void Clear(void) {
        array.clear();
      }
    };


    /** std::vector wrapper for TDriveMode elements.
     *
     * This container hides the template parameters of STL classes,
     * allowing Python wrappers to handle the elements more
     * easily. NOTE: In C++ applications, there is no sense in using
     * this container, as all functionality is already built in the
     * STL container class 'std::vector'.
     *
     * \note This could have been easily implemeted as template class
     * to allow using it in other interfaces too, but as the main goal
     * was to hide this behaviour, this class needs to be explicitly
     * defined on per type basis.
     */
    struct TDriveModeContainer {
    public:
      /** Default, empty constructor.
       *
       */
      TDriveModeContainer(void) 
        : array() {}
      

      /** Copy from TDriveMode vector.
       */
      TDriveModeContainer(const std::vector< TDriveMode > &aArray) 
        : array(aArray) {}


      /** Array of elements of type 'TCoordinate2D'.
       */
      std::vector< TDriveMode > array;
      
      
      /** Get array size in elements.
       *
       * This function returns the number of elements stored in this
       * container.
       *
       * @return                    Number of elements in the container.
       */
      inline const unsigned int GetArraySize(void) const {
        return array.size();
      }
      
      
      /** Return single array entry.
       *
       * This function returns single array entry by index.
       *
       * \note This function returns a constant reference to
       * object. Therefore, this element is only valid if the
       * underlying container (std::vector< TCoordinate2D >) remains
       * unchanged.
       * 
       * @return Constant reference to single array entry.
       *
       */
      inline const TDriveMode &GetArrayElement(const unsigned int aIndex) {
        return array[aIndex];
      }


      /** Clear associated array.
       */
      inline void Clear(void) {
        array.clear();
      }
    };

    
    class CCoordinateDriveClient;
    
    /** Callback interface class for CoordinateDrive interface.
     *
     * This interface defines functions which can be used for
     * CoordinateDriveClient callbacks. These functions are executed (more
     * detail per function basis) as new data arrives, and directly
     * from the context of the receiving thread. <b>Therefore, it is
     * important that the handling function is as simple and fast as
     * possible, as it will block the execution of the receiving
     * thread while executed.</b>
     */
    class CCoordinateDriveClientCallback
    {
      public:

      CCoordinateDriveClientCallback() {}
      virtual ~CCoordinateDriveClientCallback() {}


     /** Handler function for Point arrival events.
      *
      * This function is called when the CoordinateDriveClient receives new
      * data from the server.
      *
      * \note <b>Keep this function simple & fast as it will block the 
      * data receiving thread while executing!</b>
      *
      * @param[in] aData       Constant reference to CCoordinateDriveData element 
      *                        containing the received event.
      * @param[in] aDataSequence Sequence number of the received data. 
      *                        This should increment by one on each call.
      * @param[in] aCoordinateDriveClient Reference to CoordinateDriveClient 
      *                        instance which received this event.
      * @param[in] aUserPtr    User pointer.
      */
      virtual void OnArrivedToPointEvent(const CCoordinateDriveData &aData, 
                                       const unsigned int aDataSequence,
                                       CCoordinateDriveClient &aCoordinateDriveClient,
                                       void * aUserPtr = NULL) = 0;
      
      /** Handler function for Coordinate events.
       *
       * This function is called when the CoordinateDriveClient receives new
       * Coordinate data from the server.
       *
       * \note <b>Keep this function simple & fast as it will block the 
       * data receiving thread while executing!</b>
       *
       * @param[in] aData       Constant reference to CCoordinateDriveData element 
       *                        containing the received event.
       * @param[in] aDataSequence Sequence number of the received data. 
       *                        This should increment by one on each call.
       * @param[in] aCoordinateDriveClient Reference to CoordinateDriveClient 
       *                        instance which received this event.
       * @param[in] aUserPtr    User pointer.
       */
      virtual void OnCoordinateEvent(const CCoordinateDriveData &aData, 
                                         const unsigned int aDataSequence,
                                         CCoordinateDriveClient &aCoordinateDriveClient,
                                         void * aUserPtr = NULL) = 0;

      /** Handler function for Server status messages.
       *
       * This function is called when the CoordinateDriveClient receives new
       * Server status message from the server.
       *
       * \note <b>Keep this function simple & fast as it will block the 
       * data receiving thread while executing!</b>
       *
       * @param[in] aData       Constant reference to CCoordinateDriveData element 
       *                        containing the received event.
       * @param[in] aDataSequence Sequence number of the received data. 
       *                        This should increment by one on each call.
       * @param[in] aCoordinateDriveClient Reference to CoordinateDriveClient 
       *                        instance which received this event.
       * @param[in] aUserPtr    User pointer.
       */
      virtual void OnServerStatusMessage(const CCoordinateDriveData &aData,
                                         const unsigned int aDataSequence,
                                         CCoordinateDriveClient &aCoordinateDriveClient,
                                         void * aUserPtr = NULL) = 0;
    };
    
    /** MaCI - CoordinateDrive Client interface.
     * 
     * This class contains the methods and types for using
     * a MaCI::CoordinateDrive service in the GIMnet.
     * 
     * 
     */
    class CCoordinateDriveClient : private gim::CSync,
				   private gim::CThread,
                                   public MaCI::CMaCI
    {
    public:

      /** Constructor.
       * 
       * Constructor of the CoordinateDriveClient. This requires;
       * a pointer to Initialized and connected GIMI, minor
       * number of this interface (Used as GIMI minor number)
       *
       * @param[in] aGIMIPtr Pointer to GIMI used for communication
       * @param[in] aInterfaceMinor Interface minor number. Usually
       *            used to indicate the instance of devices of same
       *            type.
       */     
      CCoordinateDriveClient(gimi::GIMI *aGIMIPtr, const int aInterfaceMinor=-1);

      
      /** Destructor.
       */
      ~CCoordinateDriveClient();
      

      /** Open client interface.
       */
      EMaCIError DoOpen(void);


      /** Close client interface.
       */
      EMaCIError DoClose(void);

      /**
       * Sends a new coordinate to coordinateDrive server. 
       * 
       * @param[in] aX                   The x coordinate(in meters)
       * @param[in] aY                   The y coordinate(in meters)
       * @param[in] aA                   The angle of the robot at the end. 
       *                                 NOTE: This is used by coordinateDrivewhen 
       *                                 the mode is KModeUseAngle.
       * @param[in] aIsDifferential      True if the given coordinate is differential,
       *                                 False if it's global
       * @param[in] aIsPathTargetPoint   Define if the coordinate is a path or normal target point. To 
       *                                 drive the path target point the drive mode has to set
       *                                 to KModePathDriving.
       * @param[in] aIsWaitPoint         True if the given coordinate is a point where to wait
       *                                 for continue command, false if it's not. 
       *                                 NOTE: This is used when the coordinateDrive mode
       *                                 is KModePathDriving
       * @return                         False if command sending failed, true on success
       * 
       */
      bool SetCoordinate(float aX = 0.0, 
			 float aY = 0.0, 
			 float aA = 0.0,
			 bool aIsDifferential = false,
			 bool aIsPathTargetPoint = false,
			 bool aIsWaitPoint = false);

      /**
       * Sends a new coordinate to coordinateDrive server. 
       * 
       * @param aCoordinate              Coordinate to set
       * @return                         False if command sending failed, true on success
       */
      bool SetCoordinate(const TCoordinate2D &aCoordinate);

      /**
       * Removes all the coordinates, normal AND path coordinates
       * 
       * @return                      False if command sending failed, true on success
       */
      bool RemoveCoordinates();

      /**
       * Starts to drive toward the first coordinate of the list
       *
       * @return                      False if command sending failed, true on success
       */
      bool SetStart();

      /**
       * Stops driving to target
       *
       * @return                      False if command sending failed, true on success
       */
      bool SetStop();

      /**
       * Continue driving when waiting in a wait point.
       *
       * @return                      False if command sending failed, true on success
       */
      bool Continue();

      /**
       * Sets driving mode. Multiple drive modes can on at the same time. 
       * 
       *
       *
       * @return                      False if command sending failed, true on success
       *
       * \see ECoordinateDriveType
       */
      bool SetDriveMode(ECoordinateDriveMode aMode);

      /**
       * Unset the wanted driving mode.
       *
       * @return                      False if command sending failed, true on success
       *
       * \see ECoordinateDriveMode
       */
      bool UnsetDriveMode(ECoordinateDriveMode aMode);

      /**
       * Get the latest point reached with coordinateDrive
       *
       * @param aCoordinate           TCoordinate2D-struct where can be found information
       *                              about the reached coordinate
       * @param aTimestamp            Timestamp of response
       * @param aSeq                  Block until greater sequence number
       *                              available.
       *                              Initially, set the pointed data to -1.
       *                              After that, don't touch the value,
       *                              but pass it on on each call. 
       * @param[in] aTimeout_ms       Timeout int ms. The function block until 
       *                              greater sequence number is available or 
       *                              timeout reached.
       * @return                      True; on success (data stored)
       *                              False; on failure (no data stored
       *                              or partial)
       *
       * \see CCoordinateDriveData
       */
      bool GetLatestPointReached(TCoordinate2D &aCoordinate,
                                 MaCI::Common::TTimestamp &aTimestamp,
				 int *aSeq = NULL,
				 int aTimeout_ms = -1);

      /**
       * Gives all the normal coordinates that hasn't been reached already and
       * all the path coordinates. 
       *
       * @param[out] aArray           The array of coordinates is stored here.
       *                              The array is cleared before filling new information
       * @param aTimestamp            Timestamp of response
       * @param aSeq                  Block until greater sequence number
       *                              available.
       *                              Initially, set the pointed data to -1.
       *                              After that, don't touch the value,
       *                              but pass it on on each call. 
       * @param[in] aTimeout_ms       Timeout value. If the operation is not completed
       *                              before timeout is reached, the operation fails.
       * @return                      True if succeeded, false if failed(timeout, no connection..)
       */
      bool GetCoordinates(TCoordinateArray &aArray,
                          MaCI::Common::TTimestamp &aTimestamp,
                          int *aSeq = NULL,
			  unsigned int aTimeout_ms = -1);


      

      /** 
       *
       * This function acts as the overloaded base, but converts the
       * data in TCoordinateContainer format instead.
       *
       * \code
       * TCoordinateContainer cc;
       * <instance>.GetCoordinates(cc, 1000);
       * 
       * for (int i = 0; i < cc.GetArraySize(); ++i) {
       *   const TCoordinate2D &c = cc.GetArrayElement(i);
       * }
       * \endcode
       *
       * @param[out] aArray           The array of coordinates is stored here.
       *                              The array is cleared before filling new information
       * @param[out] aTimestamp       Timestamp 
       * @param[in] aSeq              Sequence number of the received data. 
       *                              Initially, set the pointed data to -1.
       *                              After that, don't touch the value,
       *                              but pass it on on each call. 
       *                      
       * @param[in] aTimeout_ms       Timeout value. If the operation is not completed
       *                              before timeout is reached, the operation fails.
       * @return                      True if succeeded, false if failed(timeout, no connection..)
       */
      inline bool GetCoordinates(TCoordinateContainer &aArray,
                                 MaCI::Common::TTimestamp &aTimestamp,
                                 int *aSeq,
                                 unsigned int aTimeout_ms);

      

      /**
       * Get server status message Event. 
       *
       * Currently possilbe server status are KCommandUnableToDriveToCoordinate,
       * KCommandPathReady and KCommandCalculating.
       *
       * KCommandUnableToDriveToCoordinate contains also the coordinate where the server
       * was unable to drive
       * 
       * KCommandPathReady is for informing that the path is ready to execute. No additional
       *                   data comes with this message.
       *       
       * KCommandCalculating is for informing that the server is calculating the path. No
       *                     additional data comes with this message.
       *
       *
       * NOTE: This is a new feature and is not supported by every server.
       *
       *
       * @param[out] aData            The Data coming from server is stored here.
       *                              The array is cleared before filling new information
       * @param[out] aTimestamp       Timestamp 
       * @param[in] aSeq              Sequence number of the received data. 
       *                              Initially, set the pointed data to -1.
       *                              After that, don't touch the value,
       *                              but pass it on on each call. 
       * @param[in] aTimeout_ms       Timeout value. If the operation is not completed
       *                              before timeout is reached, the operation fails.
       * @return                      True if succeeded, false if failed(timeout, no connection..)
       */
      bool GetServerStatusMessage(CCoordinateDriveData &aData,
                                  MaCI::Common::TTimestamp &aTimestamp,
                                 int *aSeq,
                                 unsigned int aTimeout_ms);
      /**
       * Gives length of the path left. 
       *
       * @param[out] aLinear          Sum of length of line connecting coordinates
       * @param[out] aRot             Sum of turns between orientations in points
       * @return                      True if succeeded, false if failed(timeout, no connection..)
       */
      bool GetPathLength(float *aLinear, float *aRot);

      /**
       * Gives the drivemodes that is set.
       *
       * @param[out] aArray           The array of driveModes is stored here.
       *                              The array is cleared before filling new information
       * @param[in] aTimeout_ms       Timeout value. If the operation is not completed
       *                              before timeout is reached, the operation fails.
       * @return                      True if succeeded, false if failed(timeout, no connection..)
       */
      bool GetOpenDrivingModes(TDriveModeArray &aArray,
			       unsigned int aTimeout_ms);
      /**
       * Asks for a plan between 2 coordinates from the server. 
       * \note Don't expect for a good plan from every coordinateDrive-server. Ask only from servers
       *       you know that answers right plans for this query.
       *
       * @param[in] aCoordinateFrom   Coordinate where the plan should begin.
       * @param[in] aCoordinateTo     Coordinate where the plan should end.
       * @param[out] aPlan            Plan is stored here if the server responses. The array is
       *                              cleared before filling new information.
       * @param[in] aTimeout_ms       Timeout value. If the operation is not completed
       *                              before timeout is reached, the operation fails.
       * @return                      True if succeeded, false if failed(timeout, no connection..)
       */      
      bool GetPlan(const TCoordinate2D &aCoordinateFrom,
                   const TCoordinate2D &aCoordinateTo,
                   TCoordinateArray &aPlan,
                   unsigned int aTimeout_ms);
      /** 
       *
       * This function acts as the overloaded base, but converts the
       * data in TCoordinateContainer format instead.
       *
       * \code
       * TCoordinate2D from(1,1,1)
       * TCoordinate2D to(2,2,2)
       * TCoordinateContainer cc;
       * <instance>.GetPlan(from,to,cc, 1000);
       * 
       * for (int i = 0; i < cc.GetArraySize(); ++i) {
       *   const TCoordinate2D &c = cc.GetArrayElement(i);
       * }
       * \endcode
       *
       *
       * @param[in] aCoordinateFrom   Coordinate where the plan should begin.
       * @param[in] aCoordinateTo     Coordinate where the plan should end.
       * @param[out] aPlan            Plan is stored here if the server responses. The array is
       *                              cleared before filling new information.
       * @param[in] aTimeout_ms       Timeout value. If the operation is not completed
       *                              before timeout is reached, the operation fails.
       * @return                      True if succeeded, false if failed(timeout, no connection..)
       */
      inline bool GetPlan(const TCoordinate2D &aCoordinateFrom,
                          const TCoordinate2D &aCoordinateTo,
                          TCoordinateContainer &aPlan,
                          unsigned int aTimeout_ms);       
     
      /** \overload bool GetOpenDrivingModes(TDriveModeContainer &aArray, unsigned int aTimeout_ms);
       *
       * This function acts as the overloaded base, but converts the
       * data in TDriveModeContainer format instead.
       *
       * @param[out] aArray           The array of DriveModes is stored here.
       *                              The array is cleared before filling new information
       * @param[in] aTimeout_ms       Timeout value. If the operation is not completed
       *                              before timeout is reached, the operation fails.
       * @return                      True if succeeded, false if failed(timeout, no connection..)
       */
      inline bool GetOpenDrivingModes(TDriveModeContainer &aArray,
                                      unsigned int aTimeout_ms);

      /** 
       * Set coordinateDrive driving speed
       * 
       * This function sets driving speed 
       *
       * @param[in] aSpeed            Speed to set
       * 
       * @return                      True if succeeded, false if failed(timeout, no connection..)
       */
      bool SetDrivingSpeed(const TSpeed aSpeed);

      /**
       * Gets the maximum driving speed to set
       *
       * @param[out] aMaxDrivingSpeed Maximum driving speeds are stored here
       * @param[in] aTimeout_ms       Timeout time for this call.
       *
       * @return                      True if succeeded, false if failed(timeout, no connection..)
       */
      bool GetMaximumDrivingSpeed(TSpeed &aMaxDrivingSpeed,
                                  unsigned int aTimeout_ms);

      /** Assigns the CoordinateDrive client callback function. 
       *
       * This sets a function object for calling a CoordinateDrive client
       * callback. If this is set, the interface calls the assigned
       * functions immediately when event is available.
       * 
       * \note As the function is run in the context of the receiving
       * thread, user must make sure that the callback function is
       * fast. As if the function executes too long, receiving events
       * is interrupted.
       *
       * @param[in] aCoordinateDriveClientCallback Pointer to class implementing the interface
       *                                    CCoordinateDriveClientCallback.
       * @param[in] aUserPtr User Pointer.
       */
      void SetCoordinateDriveClientCallback(CCoordinateDriveClientCallback *aCoordinateDriveClientCallback, void * aUserPtr = NULL) {
        iCoordinateDriveClientCallback = aCoordinateDriveClientCallback;
        iCallBackUserPtr = aUserPtr;
      }
      
       /** Returns the client callback pointer e.g. for destructing
       */
      CCoordinateDriveClientCallback *GetCoordinateDriveClientCallBack(void);
      
   

  private:
      /// Thread runner function. Is in charge of receiving events.
      int ThreadFunction(const int aThreadNumber);

      int ProcessMessage(const gimi::GIMIMessage &msg); ///< Incoming data processor function

      /**
       * Gets coordinate by polling
       *
       */
      bool GetCoordinates(unsigned int aTimeout_ms = -1);

      //      std::map<int,CCoordinateDriveData *> iReplyMessages;
      CCoordinateDriveData iLastReachedPoint;
      CCoordinateDriveData iCoordinates;
      CCoordinateDriveData iServerStatusData;

      int iLatestPointSeq;
      int iCoordinatesSeq;
      int iServerStatusSeq;

      std::map<int,CCoordinateDriveData *> iReplyMessages;
      int iCmdSequence;
      CCoordinateDriveClientCallback *iCoordinateDriveClientCallback;
      void *iCallBackUserPtr;
      enum ECoordinateEvent { 
        KEventPointReached = 0, ///< Point Reached
	KReplyCoordinates = 1, ///< Get Coordinate reply
	KReplyDriveModes = 2, ///< Get driving modes reply
        KEventCoordinates = 3,  ///< Coordinates event
        KEventServerStatus = 4
      };
      // Private copy const.

      CCoordinateDriveClient(const CCoordinateDriveClient &) 
        :  CSync(), 
           CThread(),
           CMaCI(NULL, 0, 0,""),
           iLastReachedPoint(),
           iCoordinates(),
           iServerStatusData(),
           iLatestPointSeq(-1),
           iCoordinatesSeq(-1),
           iServerStatusSeq(-1),
           iReplyMessages(),
           iCmdSequence(0),
           iCoordinateDriveClientCallback(NULL),
           iCallBackUserPtr(NULL){}
      CCoordinateDriveClient &operator=(const CCoordinateDriveClient &) { return *this; }

    };
    
    inline bool CCoordinateDriveClient::GetCoordinates(TCoordinateContainer &aArray,
                                                       MaCI::Common::TTimestamp &aTimestamp,
                                                       int *aSeq,
                                                       unsigned int aTimeout_ms) {
      TCoordinateArray ca;
      bool result = GetCoordinates(ca, aTimestamp,aSeq,aTimeout_ms);
      if (result) aArray = ca;
      return result;
    }
    inline bool CCoordinateDriveClient::GetOpenDrivingModes(TDriveModeContainer &aArray,
                                                            unsigned int aTimeout_ms) {
      TDriveModeArray da;
      bool result = GetOpenDrivingModes(da, aTimeout_ms);
      if (result) aArray = da;
      return result;
    }

    inline bool CCoordinateDriveClient::GetPlan(const TCoordinate2D &aCoordinateFrom,
                                                const TCoordinate2D &aCoordinateTo,
                                                TCoordinateContainer &aPlan,
                                                unsigned int aTimeout_ms){
      TCoordinateArray array;
      bool result = GetPlan(aCoordinateFrom,aCoordinateTo,array,aTimeout_ms);
      if(result) aPlan = array;
      return result;

    }
    
  }
}
#endif ///< _MACI_INTERFACE_COORDINATEDRIVECLIENT_HPP_
